inittoolbox; % Initializes open SLAM
dev = instrhwinfo('Bluetooth');
dev.RemoteNames;
bt = Bluetooth('RN42-0257', 1); % Create Bluetooth object
fopen(bt); % Connect
flushinput(bt);
fwrite(bt, '61');
connect = fscanf(bt);
if strcomp('Connection Successful', connect) == 1% Tests it connection Successful
    % Navigation Begin %
    
    %-------------------------- Robot Setup ---------------------------%
    % Robot Setup %
    params.robot.name = 'R2D2';
    params.robot.class = 'robotdd'; % Representation of robot in drawing is a differential drive robot
                                    % May need to add our own later %
    params.robot.formtype = 3; % "a round shaped robot with a line at theta"
    
    % Wheel Bases (Only one robot class. Need to mimic aerial robot.)%
    params.robot.b = 0;
    params.robot.rl = 0;
    params.robot.rr = 0;
    
    % Inital Starting position %
    params.robot.x = zeros(3,1);
    params.robot.C = 0.0001*eye(3);
    
    %-------------------------- Sensor Setup ---------------------------%
    % LiDAR %
    params.sensor1.name = 'lidar';
    % full file name and label to look for - Find a way to get in real time
    params.sensor1.datafile = 'lidar.scn';
    params.sensor1.label = 'S';
    % temporal and spatial downsampling factor - Optional
    params.sensor1.tdownsample = 5;% Every nth entry read in
    params.sensor1.sdownsample = 1;% Every nth measurement of the same entry read in
    % index string
    params.sensor1.indexstr = '1,2,3,4:2:end,5:2:end';
    % feature extraction m-file
    params.sensor1.extractionfnc = 'extractlines';
    % maximal perception radius of sensor in [m]
    params.sensor1.rs = 30.0;
    % constant range uncertainty in [m]
    params.sensor1.stdrho = 0.03;
    % robot-to-sensor transform expressed in the
    % robot frame with units [m] [m] [rad]
    params.sensor1.xs = [0; 0; 0];

    % ----- Master Sensor Setting ----- %
    % define master sensor
    params.mastersensid = 1;
    
    % ----- Feature Extraction ----- %
% size of sliding window
params.windowsize = 11; % in number of points
% threshold on model fidelity
params.threshfidel = 0.2;
% significance level for line fusion
params.fusealpha = 0.9999; % between 0 and 1
% minimal length a segment must have to be accepted
params.minlength = 0.75; % in [m]
% heuristic compensation factors for raw data correlations
params.compensa = 1*pi/180; % in [rad]
params.compensr = 0.01; % in [m]
% are the scans cyclic?
params.cyclic = 1; % 0: non-cyclic or 1: cyclic
% ----- Data Association ----- %
% significance level for NNSF matching
params.alpha = 0.999;
% ----- Slam ----- %
% optional axis vector for global map figure. Useful with infinite lines
params.axisvec = [-14 24 -19 13];
end

%If real time transfer straight into matlab use hold on
